#include "smart_car.h"


SmartCar::SmartCar(int leftMotorForwardPin, int leftMotorBackwardPin, 
int rightMotorForwardPin, int rightMotorBackwardPin, int buzzerPin, int sonicButton,int supersonicRx, int supersonicTx , int sonicRxA,int sonicTxA,int servo,bool flag_sonic):
left(leftMotorForwardPin,leftMotorBackwardPin),
right(rightMotorForwardPin, rightMotorBackwardPin),
buzzer(buzzerPin),
supersonic(supersonicRx, supersonicTx,sonicButton,sonicRxA,sonicTxA),
ultrasonic(sonicRxA,sonicTxA,sonicButton,servo)
{
}
void SmartCar::init(){
	left.init();
	right.init();
	buzzer.init();
	if (flag_sonic){
		supersonic.init();
	}
	else{
		ultrasonic.init();
	}
}


void SmartCar::turnLeftBoth(float time){
	
	right.setSpeed(40,1);
	left.setSpeed(10,1);
	if (time!=0){
		delay(time*1000);
	}
}
void SmartCar::turnRightBoth(float time){
	
	right.setSpeed(10,1);
	left.setSpeed(30,1);
	if (time!=0){
		delay(time*1000);
	}
}

void SmartCar::turnLeftInPlace(float time){
	
	right.setSpeed(80,1);
	left.setSpeed(0,0);
	if (time!=0){
		delay(time*1000);
	}
}
void SmartCar::turnRightInPlace(float time){
	
	right.setSpeed(0,0);
	left.setSpeed(75,1);
	if (time!=0){
		delay(time*1000);
	}
}

void SmartCar::backLeftInPlace(float time){
	
	right.setSpeed(0,0);
	left.setSpeed(speed_left_back,-1);
	if (time!=0){
		delay(time*1000);
	}
}
void SmartCar::backRightInPlace(float time){
	
	right.setSpeed(speed_right_back,-1);
	left.setSpeed(0,0);
	if (time!=0){
		delay(time*1000);
	}
}

void SmartCar::turnLeftReverse(float time){
	
	right.setSpeed(100,1);
	left.setSpeed(75,-1);
	if (time!=0){
		delay(time*1000);
	}
}
void SmartCar::turnRightReverse(float time){
	
	right.setSpeed(0,-1);
	left.setSpeed(100,1);
	if (time!=0){
		delay(time*1000);
	}
}

void SmartCar::run(float time){
	right.setSpeed(speed_right_run,1);
	left.setSpeed(speed_left_run,1);
	if (time!=0){
		delay(time*1000);
	}
}

void SmartCar::back(float time){
	
	right.setSpeed(speed_right_back,-1);
	left.setSpeed(speed_left_back,-1);
	if (time!=0){
		delay(time*1000);
	}
}

void SmartCar::stop(float time){
	right.setSpeed(0,0);
	left.setSpeed(0,0);
	if (time!=0){
		delay(time*1000);
	}
}


float SmartCar::getObstacleDistance(){
	return ultrasonic.getDistance();
}

void SmartCar::initspeed(float a,float b,float c,float d){
	speed_left_run=a;
	speed_right_run=b;
	speed_left_back=c;
	speed_right_back=d;
}

void SmartCar::servoPulse(int myangle){
	ultrasonic.servoPulse(myangle);
}
